#light

/// Math library for the CG system.  
/// Most of the methods have an inlined, and non-inlined version
/// so that I can test them from C# function by function.
/// The methods are designed to take different getters and setters
/// for the different types.  This is in response to a fsharp journal
/// article where they found that optimization worked very well
/// using inlined getters and setters for lu factorization.

module CGraph.CGMath
open Microsoft.FSharp.Math


/// ===================================================
/// Wrappers for commonly used functions
/// ===================================================

let ccomp = Complex.Create
let thirdroot value = System.Math.Pow( value, (1.0/3.0))
let sqrt = System.Math.Sqrt
let acos = System.Math.Acos
let cos = System.Math.Cos
let sin = System.Math.Sin
let atan2 = System.Math.Atan2
let pi = System.Math.PI; 
let dsign (b:double) = double(System.Math.Sign b)
let fabs (d:double) = System.Math.Abs d


/// ===================================================
/// Solvers
/// ===================================================


/// ax + b = 0
let SolveLinear (a:double) (b:double) =
    if a = 0.0 
        then [||] :> Complex seq
        else
            let result = -(b/a)
            [|(ccomp( result, 0.0 ))|] :> Complex seq

/// ax^2 + bx + c = 0
/// I found this on the internet, and I liked it so I converted it.
/// http://www.akiti.ca/Quad2Deg.html
let SolveQuadratic (a:double) (b:double) (c:double) =
    match a with
        | _ when a = 0.0 -> SolveLinear b c
        | _ when c = 0.0 -> 
            let single = Seq.singleton( ccomp(0.0, 0.0) )
            let linear = SolveLinear a b 
            Seq.append single linear
        | _ when b = 0.0 ->
            let dum1 = -c/a;
            if  dum1 < 0.0 
                then
                    let dum1 = -dum1;
                    let dum1 = sqrt(dum1);
                    [|ccomp(0.0,-dum1); ccomp(0.0,dum1)|] :> seq<Complex>
                else
                    let dum1 = sqrt(dum1);
                    [|ccomp(dum1,0.0);ccomp(-dum1,0.0)|] :> seq<Complex>
        | _ ->
            let b = b / (2.0*a);
            let c = c / a;
            let discrim = -c + b*b;
            if discrim < 0.0 
                then
                    let discrim = sqrt(-discrim);
                    [|ccomp(-b,discrim);ccomp(-b,-discrim)|] :> seq<Complex>
                else
                    let a =
                        if b < 0.0
                            then 
                                -b + sqrt(discrim)
                            else
                                -(b + sqrt(discrim))
                    [|ccomp(c/a, 0.0);ccomp(a,0.0)|] :> seq<Complex>
         
             
/// ax^3 + bx^2 + cx + d = 0
/// I found this on the internet, and I liked it so I converted it.
/// http://www.akiti.ca/Quad3Deg.html
let SolveCubic a b c d = 
    match a with
        | _ when a = 0.0 -> SolveQuadratic b c d
        | _ when d = 0.0 -> 
            let single = Seq.singleton( ccomp( 0.0, 0.0 ) )
            let quad = SolveQuadratic a b c
            Seq.append single quad
        | _ ->
            let b = b/a
            let c = c/a
            let d = d/a
            let q = (3.0*c - (b*b))/9.0;
            let r = -(27.0*d) + b*(9.0*c - 2.0*(b*b));
            let r = r / 54.0;
            let discrim = q*q*q + r*r;
            
            //The first root is always real.
            let term1 = (b/3.0);
            match discrim with
                | discrim when discrim > 0.0 ->
                    let s = r + sqrt(discrim)
                    let s = if s < 0.0 then -thirdroot(-s) else thirdroot(s);
                    let t = r - sqrt(discrim);
                    let t = if t < 0.0 then -thirdroot(-t) else thirdroot(t);
                    let r1 = -term1 + s + t;
                    let term1 = term1 + (s + t)/2.0;
                    let r2 = -term1
                    let r3 = -term1
                    let term1 = sqrt(3.0)*(-t + s)/2.0;
                    [|ccomp(r1, 0.0);ccomp( r2, term1);ccomp(r3,-term1)|] :> seq<Complex>
                | discrim when discrim = 0.0 ->
                    let r13 = if r < 0.0 then -thirdroot(-r) else thirdroot(r);
                    let r1 = -term1 + 2.0*r13;
                    let r2 = -(r13 + term1)
                    let r3 = r2
                    [|ccomp(r1, 0.0);ccomp( r2, 0.0);ccomp(r3,0.0)|] :> seq<Complex>
                | _ ->
                    let q = -q;
                    let dum1 = q*q*q;
                    let dum1 = acos(r/sqrt(dum1));
                    let r13 = 2.0*sqrt(q);
                    let r1 = -term1 + r13*cos(dum1/3.0);
                    let r2 = -term1 + r13*cos((dum1 + 2.0*pi)/3.0);
                    let r3 = -term1 + r13*cos((dum1 + 4.0*pi)/3.0);
                    [|ccomp(r1,0.0);ccomp(r2,0.0);ccomp(r3,0.0)|] :> seq<Complex>
                    
/// ax^4 + bx^3 + cx^2 + dx + e = 0
/// I found this on the internet, and I liked it so I converted it.
/// http://www.akiti.ca/Quad4Deg.html
let SolveQuartic a b c d e =
    match a with
    | _ when a = 0.0 -> SolveCubic b c d e
    | _ when e = 0.0 -> 
        let single = Seq.singleton (ccomp(0.0, 0.0))
        let cubic = SolveCubic a b c d
        Seq.append single cubic
    | _ -> 
        let b = b / a;
        let c = c / a;
        let d = d / a;
        let e = e / a;
        let cb = -c;
        let cc = -4.0*e + d*b;
        let cd = -(b*b*e + d*d) + 4.0*c*e;

        let q = (3.0*cc - (cb*cb))/9.0;
        let r = -(27.0*cd) + cb*(9.0*cc - 2.0*(cb*cb));
        let r = r / 54.0;
        let discrim = q*q*q + r*r;
        let term1 = (cb/3.0);
        let y1 = 
            match a with
                | _ when discrim > 0.0 ->
                    let s = r + sqrt(discrim);
                    let s = if s < 0.0 then -thirdroot(-s) else thirdroot(s)
                    let t = r - sqrt(discrim);
                    let t = if t < 0.0 then -thirdroot(-t) else thirdroot(t);
                    -term1 + s + t;
               | _ when discrim = 0.0 ->
                    let r13 = if r < 0.0 then -thirdroot(-r) else thirdroot(r)
                    -term1 + 2.0*r13
               | _ -> 
                  let q = -q;
                  let dum1 = q*q*q;
                  let dum1 = acos(r/sqrt(dum1));
                  let r13 = 2.0*sqrt(q);
                  -term1 + r13*cos(dum1/3.0);
        
        let term1 = b/4.0;
        let sqR = -c + term1*b + y1  // R-squared
        let dum1 = -(4.0*e) + y1*y1
        let z1Im,z1Re,z2Re,RRe,RIm =
            match a with
                | _ when sqR > 0.0 ->
                    let RRe = sqrt(sqR);
                    let z1Re = -(8.0*d + b*b*b)/4.0 + b*c;
                    let z1Re = z1Re / RRe;
                    let z2Re = -z1Re;
                    (0.0,z1Re,z2Re,RRe,0.0)
                | _ when sqR = 0.0 ->
                    if dum1 < 0.0
                        then
                            let z1Im = 2.0*sqrt(-dum1);
                            (z1Im,0.0,0.0,0.0,0.0)
                        else
                           let z1Re = 2.0*sqrt(dum1);
                           (0.0,z1Re,-z1Re,0.0,0.0)
                | _ ->
                    let RIm = sqrt(-sqR);
                    let z1Im = -(8.0*d + b*b*b)/4.0 + b*c;
                    let z1Im = z1Im / RIm;
                    let z1Im = -z1Im;
                    (z1Im,0.0,0.0,0.0,RIm)
                   
        let z1Re = z1Re + -(2.0*c + sqR) + 3.0*b*term1;
        let z2Re = z2Re + -(2.0*c + sqR) + 3.0*b*term1;
        let (DRe,DIm,ERe,EIm) =
            match a with
                | _ when z1Im = 0.0 ->
                    let DRe,DIm = 
                        if z1Re >= 0.0 
                            then (sqrt(z1Re), 0.0) 
                            else (0.0, sqrt(-z1Re))
                    let ERe,EIm =
                        if z2Re >= 0.0
                            then (sqrt(z2Re), 0.0)
                            else (0.0, sqrt(-z2Re))
                    (DRe,DIm,ERe,EIm)
                | _ ->
                    let r = sqrt(z1Re*z1Re + z1Im*z1Im)
                    let r = sqrt(r)
                    let dum1 = atan2(z1Im, z1Re)
                    let dum1 = dum1 / 2.0
                    let DRe = r*cos(dum1)
                    let ERe = DRe
                    let DIm = r*sin(dum1)
                    let EIm = -DIm
                    (DRe,DIm,ERe,EIm)
        [ccomp(-term1 + (RRe + DRe)/2.0,(RIm + DIm)/2.0);
         ccomp(-(term1 + DRe/2.0) + RRe/2.0,(-DIm + RIm)/2.0);
         ccomp(-(term1 + RRe/2.0) + ERe/2.0,(-RIm + EIm)/2.0);
         ccomp(-(term1 + (RRe + ERe)/2.0),-(RIm + EIm)/2.0)] :> Complex seq
         

/// ===================================================
/// Vector Ops
/// ===================================================

/// Vector get/set abstractions
type vget = int -> double
type vset = int -> double -> unit


let inline VDot3In (aget:vget) (bget:vget) =
    let a0 = aget(0)
    let a1 = aget(1)
    let a2 = aget(2)
    let b0 = bget(0)
    let b1 = bget(1)
    let b2 = bget(2)
    a0*b0 + a1*b1 + a2*b2

let VDot3 = VDot3In
    
let inline VDot4In (aget:vget) (bget:vget) =
    let a3 = aget(3)
    let b3 = bget(3)
    (VDot3In aget bget) + a3 * b3
    
let VDot4 = VDot4In
    
let inline VLen4In (get:vget) =
    sqrt(VDot4In get get)
 
let VLen4 = VLen4In
    
let inline VLen3In (get:vget) =
    sqrt(VDot3In get get)
    
let VLen3 = VLen3In
    
let inline VCross3In (aget:vget) (bget:vget) (set:vset) =
    let va0 = aget 0
    let va1 = aget 1
    let va2 = aget 2
    let vb0 = bget 0
    let vb1 = bget 1
    let vb2 = bget 2
    set 0 (va1*vb2 - va2*vb1)
    set 1 (va2*vb0 - va0*vb2)
    set 2 (va0*vb1 - va1*vb0)
    
let VCross3 = VCross3In

let inline VNorm4In (aget:vget) (set:vset) =
    let ax = aget 0
    let ay = aget 1
    let az = aget 2
    let aw = aget 3
    let len = sqrt(ax*ax + ay*ay + az*az + aw*aw)
    set 0 (ax / len)
    set 1 (ay / len)
    set 2 (az / len)
    set 3 (aw / len)
    len

let VNorm4 = VNorm4In


let inline VNorm3In (aget:vget) (set:vset) =
    let ax = aget 0
    let ay = aget 1
    let az = aget 2
    let len = sqrt(ax*ax + ay*ay + az*az)
    set 0 (ax / len)
    set 1 (ay / len)
    set 2 (az / len)
    len

let VNorm3 = VNorm3In

/// ===================================================
/// Matrix Ops
/// ===================================================

/// Matrix get/set abstractions
/// First index is the column, second index is the row
type mget = int -> int -> double

/// First index is the column, second index is the row
type mset = int -> int -> double -> unit

/// Swap rows a and b
type mswaprow = int -> int -> unit
         
let inline ExplicitDeterminant2x2In (a:double) (b:double) 
                                  (c:double) (d:double) =
    a*d - b*c
  
let ExplicitDeterminant2x2 = ExplicitDeterminant2x2In
         
let Determinant2x2In (get:mget) =
    let a = get 0 0
    let b = get 0 1
    let c = get 1 0
    let d = get 1 1
    ExplicitDeterminant2x2In a b c d

let Determinant2x2 = Determinant2x2In

let inline ExplicitDeterminant3x3In (a:double) (b:double) (c:double) 
                                  (d:double) (e:double) (f:double) 
                                  (g:double) (h:double) (i:double) =
    a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g
    
let ExplicitDeterminant3x3 = ExplicitDeterminant3x3In
    

let inline Determinant3x3In (get:mget) =
    let a = get 0 0
    let b = get 0 1
    let c = get 0 2
    let d = get 1 0
    let e = get 1 1
    let f = get 1 2
    let g = get 2 0
    let h = get 2 1
    let i = get 2 2
    ExplicitDeterminant3x3In a b c d e f g h i

let Determinant3x3 = Determinant3x3In
 
let inline Invert2x2In get set =
    let det = Determinant2x2In get
    if det <> 0. 
        then 
            let inline nget col row = -(get col row)
            set 0 0 ((get 1 1)/det)
            set 0 1 ((nget 0 1)/det)
            set 1 0 ((nget 1 0)/det)
            set 1 1 ((get 0 0)/det)
    det
  
let Invert2x2 = Invert2x2In

let inline Invert3x3In (get:mget) set =
    let inline getidx pos iter =
        if pos >= iter then (pos + 1)%3 else pos
        
    let inline innerGet iterc iterr posc posr =
        let posc = getidx posc iterc
        let posr = getidx posr iterr
        get posc posr
        
    let det = Determinant3x3In get
    if det <> 0.
        then
            let mutable mult = -1.
            for iterc in 0..2 do
                for iterr in 0..2 do
                    let innerGet = innerGet iterc iterr
                    mult <- mult * -1.
                    let value = mult * ((Determinant2x2In innerGet) / det)
                    set iterc iterr value
    det
    
let Invert3x3 = Invert3x3In

///Add a multiple of r1 to r2 and store result in set.
let inline addrow numRows (row1:vget) (row2:vget) multiple (set:vset) rowOffset =
    for row in rowOffset..numRows do
        let r1 = (row1 row)
        let r2 = (row2 row)
        set row (r2 + multiple*r1)

/// Multiply a row by a given value and store in set
let inline multrow numRows (row1:vget) multiple rowOffset (set:vset) =
    for row in rowOffset..numRows do
        let rowVal = (row1 row)
        set row (rowVal * multiple)
     
let inline elimRowSpecific elimVal rowOffset numRows (row1:vget) (row2:vget) (set:vset) =
    let r2val = row2 rowOffset
    let multiple = -r2val / elimVal
    addrow numRows row1 row2 multiple set rowOffset
    
let inline elimRow elimVal col temp numRows (get:mget) (set:mset) =
    let r1get = get col
    let r2get = get temp
    let r2set = set temp
    elimRowSpecific elimVal col numRows r1get r2get r2set

/// Perform forward elimination.  Returns true if all pivots were non-zero.
/// false otherwise.
let inline ForwardEliminationIn numCols numRows (get:mget) (set:mset) (swap:mswaprow) =
    let numCols = numCols-1
    let numRows = numRows-1
    let inline maxRowValue col =
        let mutable currentMax = System.Double.MinValue
        let mutable pivotRow = col
        //// Find the pivot row
        for temp in col..numCols do
            let rowval = fabs(get temp col)
            if rowval > currentMax 
                then 
                    currentMax <- rowval
                    pivotRow <- temp
        pivotRow
    let mutable retval = true
    /// Elimination
    for col in 0..numCols do
        let pivotRow = maxRowValue col    
        if pivotRow <> col then swap pivotRow col
        let elimVal = get col col
        
        if elimVal <> 0.0 
            then 
                ///Elimination
                for temp in (col+1)..(numCols) do
                    elimRow elimVal col temp numRows get set
                let r1get = get col
                let r1set = set col
                multrow numRows r1get (1.0/elimVal) col r1set
            else
                retval <- false
    retval
    
let ForwardElimination = ForwardEliminationIn
             
let inline BackSubstitutionIn numCols numRows (get:mget) (set:mset) =
    let numCols = numCols-1
    let numRows = numRows-1
    /// Back substitution
    for rcol in 0..(numCols-1) do
        let col = numCols - rcol
        let elimVal = get col col
        if elimVal <> 0. 
            then
                let r1get = get col
                for temp in 0..(col-1) do
                    elimRow elimVal col temp numRows get set
                    
let BackSubstitution = BackSubstitutionIn

let inline Identity4x4In (set:mset) =
    for i in 0..3 do
        for j in 0..3 do
            set i j (if i = j then 1. else 0.)
    
let Identity4x4 = Identity4x4In


/// ===================================================
/// Matrix Vector Multiply, Matrix Matrix Multiply
/// ===================================================

        
/// Ax matrix*vector -> vector
let inline VMult4In (aget:mget) (xget:vget) (set:vset) =
    for i in 0..3 do
        let avget row = aget row i 
        let result = (VDot4In xget avget)
        set i result
        
let VMult4 = VMult4In
        
/// AB matrix multiply
let inline MMult4In (aget:mget) (bget:mget) (set:mset) =
    for i in 0..3 do
        let set idx = set i idx
        let aget idx = aget i idx
        VMult4In bget aget set
       
let MMult4 = MMult4In


/// ===================================================
/// Quaternion Operations
/// ===================================================

let inline QuatConjIn (get:vget) (set:vset) =
    let inline nget idx = - (get idx)
    set 0 (nget 0)
    set 1 (nget 1)
    set 2 (nget 2)
    set 3 (get 3)
  
let QuatConj = QuatConjIn

let inline QuatMulIn (qLget:vget) (qRget:vget) (set:vset) =
    let qLx = qLget 0
    let qLy = qLget 1
    let qLz = qLget 2
    let qLw = qLget 3
    
    let qRx = qRget 0
    let qRy = qRget 1
    let qRz = qRget 2
    let qRw = qRget 3
    
    let qqw = qLw*qRw - qLx*qRx - qLy*qRy - qLz*qRz;
    let qqx = qLw*qRx + qLx*qRw + qLy*qRz - qLz*qRy;
    let qqy = qLw*qRy + qLy*qRw + qLz*qRx - qLx*qRz;
    let qqz = qLw*qRz + qLz*qRw + qLx*qRy - qLy*qRx;
    
    set 0 qqx
    set 1 qqy
    set 2 qqz
    set 3 qqw
    
let QuatMul = QuatMulIn


let inline QuatFromAxisAngleIn (axis:vget) angle (set:vset) =
    let halfAngle = angle / 2.
    let sinAngle = sin halfAngle
    let axisx = (axis 0)
    let axisy = (axis 1)
    let axisz = (axis 2)
    set 0 (axisx * sinAngle)
    set 1 (axisy * sinAngle)
    set 2 (axisz * sinAngle)
    set 3 (cos( halfAngle ))
    
let QuatFromAxisAngle = QuatFromAxisAngleIn


let inline QuatToMatrixIn (quat:vget) (matrix:mset) =
    let x = quat 0
    let y = quat 1
    let z = quat 2
    let w = quat 3
    
    let sqw = w*w;
    let sqx = x*x;
    let sqy = y*y;
    let sqz = z*z;

    // invs (inverse square length) is only required if quaternion is not already normalised
    let invs = 1. / (sqx + sqy + sqz + sqw)
    matrix 0 0 (( sqx - sqy - sqz + sqw) * invs)
    matrix 1 1 ((-sqx + sqy - sqz + sqw) * invs)
    matrix 2 2 ((-sqx - sqy + sqz + sqw) * invs)
    
    let tmp1 = x*y;
    let tmp2 = z*w;
    
    matrix 1 0 (2.0 * (tmp1 + tmp2)*invs)
    matrix 0 1 (2.0 * (tmp1 - tmp2)*invs)
    
    let tmp1 = x*z;
    let tmp2 = y*w;
    
    matrix 2 0 (2.0 * (tmp1 - tmp2)*invs)
    matrix 0 2 (2.0 * (tmp1 + tmp2)*invs)
    
    let tmp1 = y*z;
    let tmp2 = x*w;
    
    matrix 2 1 (2.0 * (tmp1 + tmp2)*invs)
    matrix 1 2 (2.0 * (tmp1 - tmp2)*invs) 
    
let QuatToMatrix = QuatToMatrixIn

// http://www.3dkingdoms.com/weekly/quat.h
let inline SlerpIn (a:vget) (b:vget) t (set:vset) =
    let cosTheta = VDot4In a b;
    let theta    = acos(cosTheta);
    let sinTheta = sin(theta);
    let w1,w2 = 
        if sinTheta > 0.0001
            then (sin( (1.0-t)*theta ) / sinTheta, sin( t*theta) / sinTheta )
            else 1.0 - t, t
    let inline dotadd idx =
        set idx ((a idx)*w1 + ((b idx)*w2))
    dotadd 0 
    dotadd 1
    dotadd 2
    dotadd 3
    
let Slerp = SlerpIn

// http://www.3dkingdoms.com/weekly/quat.h
let inline NLerpIn(a:vget) (b:vget) w2 (set:vset) =
    let w1 = 1.0 - w2
    let inline dotadd idx =
        (a idx)*w1 + (b idx)*w2
        
    // Normalize the result
    VNorm4In dotadd set
    
let NLerp = NLerpIn
        